
## if Misha connected encoders the wrong way set this to True !
#REVERTED_MOTORS = True

import struct 
from Brain.io.core import Comm

class Wheels:
	""" Describes whells peripheral
	"""
	
	LEFT_MIN_SPEED = 20
	#LEFT_MIN_SPEED = 9
	RIGHT_MIN_SPEED = 20
	#RIGHT_MIN_SPEED = 9
	LEFT_SPEED_RANGE = 1
	RIGHT_SPEED_RANGE = 1
	
	TOLERANCE = 0.05
	
	def __init__(self):
		## Power on each wheel [-1, 1]
		self.power_left = 0.0
		self.power_right = 0.0
		
		self.pipe = Comm.link('actuators')
	
	def calculate_speed_L(self, power):
		if abs(power) < self.TOLERANCE: return 0
		power = abs(power)
		ret = (power * self.LEFT_SPEED_RANGE) + self.LEFT_MIN_SPEED
		return int(ret)

	def calculate_speed_R(self, power):
		if abs(power) < self.TOLERANCE: return 0
		power = abs(power)
		ret = (power * self.RIGHT_SPEED_RANGE) + self.RIGHT_MIN_SPEED
		return int(ret)

	def write(self, power_on_left, power_on_right):
		self.power_left,  self.power_right = power_on_left, power_on_right
		
		## left
		data_l, data_r, = 0x80, 0x40, 
		
		if power_on_left >= 0.0: 
			data_l |= 0x40
		data_l |= self.calculate_speed_L(power_on_left)
		## right
		if power_on_right >= 0.0:
			data_r &= 0xBF
		data_r |= self.calculate_speed_R(power_on_right)
		
		self.pipe.write( struct.pack('<BB', data_r, data_l) )
	
	def send_raw(self, left, right):
		l = abs(int(left)) | 0x80
		if left >= 0.0: l |= 0x40 
		r = abs(int(right)) 
		if right < 0.0:  r |= 0x40 
		#print l, r 
		self.pipe.write( struct.pack('<BB', r, l) )
	
	def stop(self):
		self.pipe.write( struct.pack('<BB', 0x80, 0x00) )

if __name__ == '__main__':
	import time
	p = Wheels()
	print 'go go go ..'
	p.write(0.0, 1.0)
	time.sleep(3.0)
	p.write( 0.0, -1.0)
	time.sleep(3.0)
	p.write(0.0, 1.0)
	time.sleep(3.0)
	p.write( 0.0, -1.0)
	time.sleep(3.0)
	#p.write(-1.0, -1.0)
	#time.sleep(10.0)
	p.write(0, 0)
	time.sleep(0.1)
	
	




















